#include <fstream>
#include <ostream>
#include <math.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/ModelCoefficients.h>

#include "ModelController.h"
#include "Defines.h"


ModelController::ModelController() :
   cloud(new pcl::PointCloud<pcl::PointXYZRGB>)
{
      
};

ModelController::~ModelController()
{
   this->shutDown();
};

int ModelController::init()
{
   if(!cloud.get())
      return NO_SUCCESS;
   
   return SUCCESS;
};

int ModelController::shutDown()
{
   return SUCCESS;
};

int ModelController::generateModel()
{
   /*
   pcl::ModelCoefficients::Ptr plate_coefficients(new pcl::ModelCoefficients);
   pcl::PointIndices::Ptr plate_inliers(new pcl::PointIndices);
   
   pcl::SACSegmentation<pcl::PointXYZRGB> segmentation;

   segmentation.setOptimizeCoefficients(true);

   segmentation.setModelType(pcl::SACMODEL_PLANE);
   segmentation.setMethodType(pcl::SAC_RANSAC);
   segmentation.setDistanceThreshold(0.005);

   segmentation.setInputCloud(plate);
   segmentation.segment(*plate_inliers, *plate_coefficients);

   if(0==plate_inliers->indices.size())
      return NO_SUCCESS;

   float a = plate_coefficients->values[0];
   float b = plate_coefficients->values[1];
   float c = plate_coefficients->values[2];
   float d = plate_coefficients->values[3];
   float x,y,z,res;

   for(unsigned int i=0; i<cloud->size(); i++) {
      x = (*cloud)[i].x;
      y = (*cloud)[i].y;
      z = (*cloud)[i].z;

      res = a*x + b*y + c*z + d;
      
      if(res<0.005) {
         (*cloud)[i].r = 255;
         (*cloud)[i].g = 0;
         (*cloud)[i].b = 0;
      }
   }

   pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
   pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
   
   segmentation.setModelType(pcl::SACMODEL_PARALLEL_PLANE);
   segmentation.setInputCloud(cloud);

   Eigen::Vector3f normal;
   normal[0] = plate_coefficients->values[0];
   normal[1] = plate_coefficients->values[1];
   normal[2] = plate_coefficients->values[2];
   segmentation.setAxis(normal);

   segmentation.setEpsAngle(90.0);
   std::cout << segmentation.getEpsAngle() << std::endl;

   segmentation.segment(*inliers, *coefficients);

   if(0==inliers->indices.size())
      return NO_SUCCESS;

   for(unsigned int i=0; i<inliers->indices.size(); i++) {
      (*cloud)[inliers->indices[i]].r = 255;
      (*cloud)[inliers->indices[i]].g = 0;
      (*cloud)[inliers->indices[i]].b = 0;
   }
   */
   return SUCCESS;
};

int ModelController::getModel(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& _model)
{
   *_model = *cloud;
   
   return SUCCESS;
};

int ModelController::useImages(unsigned char* _rgb, unsigned short* _depth, unsigned int _degree)
{
   return SUCCESS;
};

int ModelController::usePlyFile(const char* _file_name)
{
   std::ifstream file_stream(_file_name, std::ifstream::in);
   pcl::PointCloud<pcl::PointXYZRGB>::Ptr plate(new pcl::PointCloud<pcl::PointXYZRGB>);

   float x,y,z;
   unsigned int red,green,blue;
   pcl::PointXYZRGB point;
   double distance;

   while(file_stream.good()) {
      file_stream >> x;
      file_stream.ignore();
      file_stream >> y;
      file_stream.ignore();
      file_stream >> z;
      file_stream.ignore();
      file_stream >> red;
      file_stream.ignore();
      file_stream >> green;
      file_stream.ignore();
      file_stream >> blue;
      file_stream.ignore(2, '\n');
      
      distance = sqrt(x*x + y*y + z*z);
      if(distance < MIN_DISTANCE || distance > MAX_DISTANCE){
         continue;
      }

      if (red<110 && green>160 && blue<110){
         point.x = x;
         point.y = y;
         point.z = z;
         point.r = red;
         point.g = green;
         point.b = blue;
         plate->push_back(point);
      }
   }

   if(plate->empty()) {
      std::cout << "Empty plate pointcloud!" << std::endl;
      return NO_SUCCESS;
   }

   pcl::ModelCoefficients::Ptr plate_coefficients(new pcl::ModelCoefficients);
   pcl::PointIndices::Ptr plate_inliers(new pcl::PointIndices);
   pcl::SACSegmentation<pcl::PointXYZRGB> segmentation;

   segmentation.setOptimizeCoefficients(true);
   segmentation.setModelType(pcl::SACMODEL_PLANE);
   segmentation.setMethodType(pcl::SAC_RANSAC);
   segmentation.setDistanceThreshold(0.005);

   segmentation.setInputCloud(plate);
   segmentation.segment(*plate_inliers, *plate_coefficients);

   if(0==plate_inliers->indices.size()) {
      std::cout << "No plate found!" << std::endl;
      return NO_SUCCESS;
   }

   float a = plate_coefficients->values[0];
   float b = plate_coefficients->values[1];
   float c = plate_coefficients->values[2];
   float d = plate_coefficients->values[3];
   float res;

   file_stream.close();
   file_stream.open(_file_name, std::ifstream::in);

   while(file_stream.good()) {
      file_stream >> x;
      file_stream.ignore();
      file_stream >> y;
      file_stream.ignore();
      file_stream >> z;
      file_stream.ignore();
      file_stream >> red;
      file_stream.ignore();
      file_stream >> green;
      file_stream.ignore();
      file_stream >> blue;
      file_stream.ignore(2, '\n');
      
      distance = sqrt(x*x + y*y + z*z);
      if(distance < MIN_DISTANCE || distance > MAX_DISTANCE){
         continue;
      }

      res = a*x + b*y + c*z + d;
      if(res<0.005) {
         continue;
      }

      point.x = x;
      point.y = y;
      point.z = z;
      point.r = red;
      point.g = green;
      point.b = blue;
      this->cloud->push_back(point);      
   }

   if(this->cloud->empty()) {
      std::cout << "Empty pointcloud!" << std::endl;
      return NO_SUCCESS;
   }

   file_stream.close();

   /*
   for(unsigned int i=0; i<cloud->size(); i++) {
      x = (*cloud)[i].x;
      y = (*cloud)[i].y;
      z = (*cloud)[i].z;

      res = a*x + b*y + c*z + d;
      
      if(res<0.005) {
         cloud->erase(i + cloud->begin());
         i--;
      }
   }
   */
   
   return SUCCESS;
};